clear;
clc;

%% 如果要转成C++的话，感觉要处理很多
% 1. 解析stl文件，获取顶点和三角面
% 2. 计算均值
% 3. 如何显示顶点和面，应该就是实际的三维图
% 4. 计算法向量，计算每个面片的法向量
% 5. 

% 读取STL文件
stlFile = '3.stl'; % 替换为你的STL文件路径
% TR = stlread(filename)：返回三角剖分对象 TR，其中包含 STL 文件中定义的三角形。
geometry = stlread(stlFile);

% 提取顶点和面信息
vertices = geometry.Points; % 顶点坐标
faces = geometry.ConnectivityList; % 面的顶点索引

% 计算质心（假设为圆盘的中心）
% 计算并存储 vertices 矩阵每列的均值，从而得到模型的质心坐标。
center = mean(vertices, 1);

% 将所有顶点坐标平移，使中心为原点
vertices_centered = vertices - center;

% 显示平移后的几何体
figure;
patch('Vertices', vertices_centered, 'Faces', faces, ...
    'FaceColor', 'cyan', 'EdgeColor', 'none');
axis equal;
xlabel('X');
ylabel('Y');
zlabel('Z');
title('3D STL Model Centered at Disk Center');
view(3);
camlight;
lighting gouraud;

% 在图中标出原点
hold on;
plot3(0, 0, 0, 'ro', 'MarkerSize', 5, 'LineWidth', 2);
text(0, 0, 0, '原点', 'VerticalAlignment', 'bottom', 'HorizontalAlignment', 'right', 'FontSize', 12, 'FontWeight', 'bold', 'Color', 'red');
hold off;

grid on;

% 计算法向量，计算每个面片的法向量
normals = faceNormals(geometry);

% 设置法向量的阈值，假设孔洞的面法向量和圆盘表面有显著不同
normal_tolerance = 0.1; % 视情况调整
surface_normal = mean(normals); % 假设圆盘表面法向量为平均法向量，将计算出的平均法向量作为圆盘表面的法向量。
dot_products = normals * surface_normal'; % ' 计算两个向量的点积，可以表示相似度。
hole_faces_indices = find(abs(dot_products) < normal_tolerance); % 存储这些面片的索引。

% 提取孔洞边界点
hole_vertices_indices = unique(faces(hole_faces_indices, :)); % 函数用于去除重复的顶点索引，从而得到孔洞边界上的所有唯一顶点索引。这些顶点索引是构成孔洞边界的顶点
hole_vertices = vertices_centered(hole_vertices_indices, :); % 点。。。

% 使用K-means聚类来识别孔洞边界点
k = 12; % 假设最多有k个孔洞，根据需要调整
% 是一个向量，长度等于 hole_vertices 中的顶点数量，每个元素表示对应顶点所属的簇索引。
% 是一个矩阵，大小为 k×3，其中每行表示一个簇的中心坐标（x、y、z）。
[idx, C] = kmeans(hole_vertices, k); % 点的k-means聚类 

% 遍历每个聚类的点，拟合圆形
figure;
hold on;
colors = lines(k); % 用不同颜色显示每个孔洞
for i = 1:k

    % 计算每个聚类的点数量
    cluster_points = hole_vertices(idx == i, :);
    if size(cluster_points, 1) < 3
        continue; % 忽略点数少于3的聚类
    end

    [xc, yc, zc, R] = fitSphere(cluster_points(:, 1), cluster_points(:, 2), cluster_points(:, 3));
    
    % 画出拟合的圆,三维散点图
    plot3(cluster_points(:, 1), cluster_points(:, 2), cluster_points(:, 3), 'o', 'Color', colors(i, :));

    theta = linspace(0, 2*pi, 100);
    x_fit = xc + R * cos(theta);
    y_fit = yc + R * sin(theta);
    z_fit = zc * ones(size(x_fit)); % 简化显示圆的平面
    plot3(x_fit, y_fit, z_fit, '-', 'Color', colors(i, :), 'LineWidth', 2);
    disp(['孔洞 ' num2str(i) ' 圆心坐标: (', num2str(xc), ', ', num2str(yc), ', ', num2str(zc), ')']);
    disp(['孔洞 ' num2str(i) ' 半径: ', num2str(R)]);
end
hold off;
axis equal;

xlabel('X');
ylabel('Y');
zlabel('Z');
title('Hole Detection and Fitting');
view(3);
grid on;

% 函数：计算法向量
function normals = faceNormals(geometry)
    vertices = geometry.Points;
    faces = geometry.ConnectivityList;
    v1 = vertices(faces(:, 2), :) - vertices(faces(:, 1), :);
    v2 = vertices(faces(:, 3), :) - vertices(faces(:, 1), :);
    normals = cross(v1, v2);
    normals = normals ./ sqrt(sum(normals.^2, 2)); % 单位化
end

% 函数：拟合球面（近似为孔洞的圆）
% 利用最小二乘法拟合球
% 因为球的方程式：(x-a)^2+(y-b)^2+(z-c)^2=r^2
% -2ax -2by-2cz+1*(a^2+b^2+c^2-r^2)=-x^2-y^2-z^2
% [x y z 1][-2a -2b -2c (a^2+b^2+c^2-r^2)] = e
% 求出参数[-2a -2b -2c (a^2+b^2+c^2-r^2)]
% 代入所有的数据，可以得到矩阵
function [xc, yc, zc, R] = fitSphere(x, y, z)
    % 使用最小二乘法拟合球面
    A = [x, y, z, ones(size(x))];
    b = -(x.^2 + y.^2 + z.^2);
    coeffs = A \ b;
    xc = -0.5 * coeffs(1); % -2a=coeffs(1)
    yc = -0.5 * coeffs(2);
    zc = -0.5 * coeffs(3);
    R = sqrt((coeffs(1)^2 + coeffs(2)^2 + coeffs(3)^2) / 4 - coeffs(4));
    % a^2+b^2+c^2-r^2=coeffs(4)^2 => r^2 = a^2+b^2+c^2-coeffs(4)^2
end
